
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass_hmc5843.c
  * @author     baiyang
  * @date       2021-12-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_compass_hmc5843.h"

#include <common/console/console.h>
#include <common/utility/sparse-endian.h>
/*-----------------------------------macro------------------------------------*/
/*
 * Defaul address: 0x1E
 */

#define HMC5843_REG_CONFIG_A 0x00
// Valid sample averaging for 5883L
#define HMC5843_SAMPLE_AVERAGING_1 (0x00 << 5)
#define HMC5843_SAMPLE_AVERAGING_2 (0x01 << 5)
#define HMC5843_SAMPLE_AVERAGING_4 (0x02 << 5)
#define HMC5843_SAMPLE_AVERAGING_8 (0x03 << 5)

#define HMC5843_CONF_TEMP_ENABLE   (0x80)

// Valid data output rates for 5883L
#define HMC5843_OSR_0_75HZ (0x00 << 2)
#define HMC5843_OSR_1_5HZ  (0x01 << 2)
#define HMC5843_OSR_3HZ    (0x02 << 2)
#define HMC5843_OSR_7_5HZ  (0x03 << 2)
#define HMC5843_OSR_15HZ   (0x04 << 2)
#define HMC5843_OSR_30HZ   (0x05 << 2)
#define HMC5843_OSR_75HZ   (0x06 << 2)
// Sensor operation modes
#define HMC5843_OPMODE_NORMAL 0x00
#define HMC5843_OPMODE_POSITIVE_BIAS 0x01
#define HMC5843_OPMODE_NEGATIVE_BIAS 0x02
#define HMC5843_OPMODE_MASK 0x03

#define HMC5843_REG_CONFIG_B 0x01
#define HMC5883L_GAIN_0_88_GA (0x00 << 5)
#define HMC5883L_GAIN_1_30_GA (0x01 << 5)
#define HMC5883L_GAIN_1_90_GA (0x02 << 5)
#define HMC5883L_GAIN_2_50_GA (0x03 << 5)
#define HMC5883L_GAIN_4_00_GA (0x04 << 5)
#define HMC5883L_GAIN_4_70_GA (0x05 << 5)
#define HMC5883L_GAIN_5_60_GA (0x06 << 5)
#define HMC5883L_GAIN_8_10_GA (0x07 << 5)

#define HMC5843_GAIN_0_70_GA (0x00 << 5)
#define HMC5843_GAIN_1_00_GA (0x01 << 5)
#define HMC5843_GAIN_1_50_GA (0x02 << 5)
#define HMC5843_GAIN_2_00_GA (0x03 << 5)
#define HMC5843_GAIN_3_20_GA (0x04 << 5)
#define HMC5843_GAIN_3_80_GA (0x05 << 5)
#define HMC5843_GAIN_4_50_GA (0x06 << 5)
#define HMC5843_GAIN_6_50_GA (0x07 << 5)

#define HMC5843_REG_MODE 0x02
#define HMC5843_MODE_CONTINUOUS 0x00
#define HMC5843_MODE_SINGLE     0x01

#define HMC5843_REG_DATA_OUTPUT_X_MSB 0x03

#define HMC5843_REG_ID_A 0x0A
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_compass_hmc5843_destructor(sensor_compass_hmc5843 *hmc5843);
static bool init(sensor_compass_hmc5843 *hmc5843);
static void read(sensor_compass_backend *compass_backend);
static void _timer(void *parameter);
static bool _setup_sampling_mode(sensor_compass_hmc5843 *hmc5843);
static bool _read_sample(sensor_compass_hmc5843 *hmc5843);
static void _take_sample(sensor_compass_hmc5843 *hmc5843);
static bool _check_whoami(sensor_compass_hmc5843 *hmc5843);
static bool _calibrate(sensor_compass_hmc5843 *hmc5843);
/*----------------------------------variable----------------------------------*/
struct sensor_compass_backend_ops hmc5843_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_compass_hmc5843_ctor(sensor_compass_hmc5843 *hmc5843, gp_device_t dev, bool force_external, enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(hmc5843, 0, sizeof(sensor_compass_hmc5843));

    sensor_compass_backend_ctor(&hmc5843->backend, "hmc5843");

    hmc5843_ops = compass_backend_ops;
    hmc5843_ops.read = read;

    hmc5843->backend.ops = &hmc5843_ops;
    hmc5843->_dev = dev;

    hmc5843->_force_external = force_external;
    hmc5843->_rotation       = rotation;
}

void sensor_compass_hmc5843_destructor(sensor_compass_hmc5843 *hmc5843)
{
    sensor_compass_backend_destructor(&hmc5843->backend);
}

/**
  * @brief       
  * @param[in]   dev  
  * @param[in]   force_external  
  * @param[in]   rotation  
  * @param[out]  
  * @retval      
  * @note        
  */
sensor_compass_backend *sensor_compass_hmc5843_probe(gp_device_t dev, bool force_external, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_compass_hmc5843 *sensor = (sensor_compass_hmc5843 *)rt_malloc(sizeof(sensor_compass_hmc5843));

    if (!sensor) {
        return NULL;
    }

    sensor_compass_hmc5843_ctor(sensor, dev, force_external, rotation);

    if (!init(sensor)) {
        sensor_compass_hmc5843_destructor(sensor);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_compass_backend *)sensor;
}

static bool init(sensor_compass_hmc5843 *hmc5843)
{
    // set read and auto-increment flags on SPI
    if (devmgr_get_bus_type(hmc5843->_dev) == BUS_TYPE_SPI) {
        devmgr_set_read_flag(hmc5843->_dev, 0xC0);
    }
    
    // high retries for init
    devmgr_set_retries(hmc5843->_dev, 10);

    if (!_check_whoami(hmc5843)) {
        goto errout;
    }

    if (!_calibrate(hmc5843)) {
        console_printf("HMC5843: Could not calibrate sensor\n");
        goto errout;
    }

    if (!_setup_sampling_mode(hmc5843)) {
        goto errout;
    }

    hmc5843->_initialised = true;

    // lower retries for run
    devmgr_set_retries(hmc5843->_dev, 3);

    // perform an initial read
    read(&hmc5843->backend);

    //register compass instance
    devmgr_set_device_type(hmc5843->_dev, COMPASS_DEVTYPE_HMC5883);
    if (!sensor_compass_register_compass(devmgr_get_bus_id(hmc5843->_dev), &hmc5843->_compass_instance)) {
        return false;
    }
    sensor_compass_backend_set_dev_id(hmc5843->_compass_instance, devmgr_get_bus_id(hmc5843->_dev));

    sensor_compass_backend_set_rotation(hmc5843->_compass_instance, hmc5843->_rotation);
    
    if (hmc5843->_force_external) {
        sensor_compass_backend_set_external(hmc5843->_compass_instance, true);
    }

    // read from sensor at 75Hz
    devmgr_register_periodic_callback(hmc5843->_dev, 13333, _timer, hmc5843);

    console_printf("HMC5843 found on bus 0x%x\n", (unsigned)devmgr_get_bus_id(hmc5843->_dev));
    
    return true;

errout:
    return false;
}

/*
 * Take accumulated reads from the magnetometer or try to read once if no
 * valid data
 *
 * bus semaphore must not be locked
 */
static void read(sensor_compass_backend *compass_backend)
{
    sensor_compass_hmc5843 *hmc5843 = (sensor_compass_hmc5843 *)compass_backend;

    if (!hmc5843->_initialised) {
        // someone has tried to enable a compass for the first time
        // mid-flight .... we can't do that yet (especially as we won't
        // have the right orientation!)
        return;
    }

    sensor_compass_backend_drain_accumulated_samples(&hmc5843->backend, hmc5843->_compass_instance, &hmc5843->_scaling);
}

/*
 * take a reading from the magnetometer
 *
 * bus semaphore has been taken already by HAL
 */
static void _timer(void *parameter)
{
    sensor_compass_hmc5843 *hmc5843 = (sensor_compass_hmc5843 *)parameter;
    
    bool result = _read_sample(hmc5843);

    // always ask for a new sample
    _take_sample(hmc5843);
    
    if (!result) {
        return;
    }

    // get raw_field - sensor frame, uncorrected
    Vector3f_t raw_field = {hmc5843->_mag_x, hmc5843->_mag_y, hmc5843->_mag_z};
    vec3_mult(&raw_field, &raw_field, hmc5843->_gain_scale);

    // rotate to the desired orientation
    if (sensor_compass_backend_is_external(hmc5843->_compass_instance)) {
        vec3_rotate(&raw_field, ROTATION_YAW_90);
    }

    // We expect to do reads at 10Hz, and  we get new data at most 75Hz, so we
    // don't expect to accumulate more than 8 before a read; let's make it
    // 14 to give more room for the initialization phase
    sensor_compass_backend_accumulate_sample(&hmc5843->backend, &raw_field, hmc5843->_compass_instance, 14);
}

static bool _setup_sampling_mode(sensor_compass_hmc5843 *hmc5843)
{
    hmc5843->_gain_scale = (1.0f / 1090) * 1000;
    if (!devmgr_write_register(hmc5843->_dev, HMC5843_REG_CONFIG_A,
                              HMC5843_CONF_TEMP_ENABLE |
                              HMC5843_OSR_75HZ |
                              HMC5843_SAMPLE_AVERAGING_1, false) ||
        !devmgr_write_register(hmc5843->_dev, HMC5843_REG_CONFIG_B,
                              HMC5883L_GAIN_1_30_GA, false) ||
        !devmgr_write_register(hmc5843->_dev, HMC5843_REG_MODE,
                              HMC5843_MODE_SINGLE, false)) {
        return false;
    }
    return true;
}

/*
 * Read Sensor data - bus semaphore must be taken
 */
static bool _read_sample(sensor_compass_hmc5843 *hmc5843)
{
    // 定义一字节对齐的匿名结构体
    struct PACKED {
        be16_t rx;
        be16_t ry;
        be16_t rz;
    } val;
    int16_t rx, ry, rz;

    if (!devmgr_read_registers(hmc5843->_dev, HMC5843_REG_DATA_OUTPUT_X_MSB, (uint8_t *) &val, sizeof(val))){
        return false;
    }

    rx = be16toh(val.rx);
    ry = be16toh(val.rz);
    rz = be16toh(val.ry);

    if (rx == -4096 || ry == -4096 || rz == -4096) {
        // no valid data available
        return false;
    }

    hmc5843->_mag_x = -rx;
    hmc5843->_mag_y =  ry;
    hmc5843->_mag_z = -rz;

    return true;
}

static void _take_sample(sensor_compass_hmc5843 *hmc5843)
{
    devmgr_write_register(hmc5843->_dev, HMC5843_REG_MODE,
                         HMC5843_MODE_SINGLE, false);
}

static bool _check_whoami(sensor_compass_hmc5843 *hmc5843)
{
    uint8_t id[3];
    if (!devmgr_read_registers(hmc5843->_dev, HMC5843_REG_ID_A, id, 3)) {
        // can't talk on bus
        return false;        
    }
    if (id[0] != 'H' ||
        id[1] != '4' ||
        id[2] != '3') {
        // not a HMC5x83 device
        return false;
    }

    return true;
}

static bool _calibrate(sensor_compass_hmc5843 *hmc5843)
{
    uint8_t calibration_gain;
    int numAttempts = 0, good_count = 0;
    bool success = false;

    calibration_gain = HMC5883L_GAIN_2_50_GA;

    /*
     * the expected values are based on observation of real sensors
     */
    float expected[3] = { 1.16*600, 1.08*600, 1.16*600 };

    uint8_t base_config = HMC5843_OSR_15HZ;
    uint8_t num_samples = 0;
    
    while (success == 0 && numAttempts < 25 && good_count < 5) {
        numAttempts++;

        // force positiveBias (compass should return 715 for all channels)
        if (!devmgr_write_register(hmc5843->_dev, HMC5843_REG_CONFIG_A,
                                  base_config | HMC5843_OPMODE_POSITIVE_BIAS, false)) {
            // compass not responding on the bus
            continue;
        }

        rt_thread_mdelay(50);

        // set gains
        if (!devmgr_write_register(hmc5843->_dev, HMC5843_REG_CONFIG_B, calibration_gain, false) ||
            !devmgr_write_register(hmc5843->_dev, HMC5843_REG_MODE, HMC5843_MODE_SINGLE, false)) {
            continue;
        }

        // read values from the compass
        rt_thread_mdelay(50);
        if (!_read_sample(hmc5843)) {
            // we didn't read valid values
            continue;
        }

        num_samples++;

        float cal[3];

        // hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);

        cal[0] = fabsf(expected[0] / hmc5843->_mag_x);
        cal[1] = fabsf(expected[1] / hmc5843->_mag_y);
        cal[2] = fabsf(expected[2] / hmc5843->_mag_z);

        // hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]);

        // we throw away the first two samples as the compass may
        // still be changing its state from the application of the
        // strap excitation. After that we accept values in a
        // reasonable range
        if (numAttempts <= 2) {
            continue;
        }

#define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)

        if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
            IS_CALIBRATION_VALUE_VALID(cal[1]) &&
            IS_CALIBRATION_VALUE_VALID(cal[2])) {
            // hal.console->printf("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]);
            good_count++;

            hmc5843->_scaling.vec3[0] += cal[0];
            hmc5843->_scaling.vec3[1] += cal[1];
            hmc5843->_scaling.vec3[2] += cal[2];
        }

#undef IS_CALIBRATION_VALUE_VALID

#if 0
        /* useful for debugging */
        hal.console->printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z);
        hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]);
#endif
    }

    devmgr_write_register(hmc5843->_dev, HMC5843_REG_CONFIG_A, base_config, false);
    
    if (good_count >= 5) {
        hmc5843->_scaling.vec3[0] = hmc5843->_scaling.vec3[0] / good_count;
        hmc5843->_scaling.vec3[1] = hmc5843->_scaling.vec3[1] / good_count;
        hmc5843->_scaling.vec3[2] = hmc5843->_scaling.vec3[2] / good_count;
        success = true;
    } else {
        /* best guess */
        hmc5843->_scaling.vec3[0] = 1.0f;
        hmc5843->_scaling.vec3[1] = 1.0f;
        hmc5843->_scaling.vec3[2] = 1.0f;
        if (num_samples > 5) {
            // a sensor can be broken for calibration but still
            // otherwise workable, accept it if we are reading samples
            success = true;
        }
    }

#if 0
    printf("scaling: %.2f %.2f %.2f\n",
           _scaling[0], _scaling[1], _scaling[2]);
#endif
    
    return success;
}

/*------------------------------------test------------------------------------*/


